/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "GIMIConnector.h"
#include "J2B2.h"
#include "World.h"
#include "Protocol.h"
#define GIMI_CLIENT_API 20000
#include "gimi.h"
//#include "wx/wx.h"

CGIMIConnector::CGIMIConnector(CJ2B2 *j2b2,gimi::GIMI *gimi, std::string name)
:j2b2(j2b2),
gimi(gimi)
{
  /*
  gimi=new gimi::GIMI(1);

  if (gimi->readSettings("gimi_settings.xml"))
  {
    int r;
    r=gimi->connectToHubEx("",-1,name);
    if (r==GIMI_OK)
    {*/
  if (gimi->isConnected())
  {
    gimi->addWantedTypeId(GIMI_PROTOCOL_FSRSIM_POSITION,GIMI_IGNOREMINORTYPE);
    gimi->addWantedTypeId(GIMI_PROTOCOL_FSRSIM_MOTORCMD,GIMI_IGNOREMINORTYPE);
    gimi->addProvidedService(GIMI_PROTOCOL_FSRSIM_POSITION,0,"position");
    gimi->addProvidedService(GIMI_PROTOCOL_FSRSIM_RANGING,0,"laser");
    gimi->addProvidedService(GIMI_PROTOCOL_FSRSIM_RANGING,1,"bumper");
  }
      //dPrint(1,"Connected using name %s",name.c_str());
    /*} else if (r==GIMI_INVALIDNAME){
      wxString s=_T("Couldn't connect GIMI: Name ");
      s << name << "already in use.";
      wxMessageBox(s);
      dPrint(1,"Couldn't connect GIMI: Name '%s' already in use.", name.c_str());
    } else {
      wxMessageBox("Couldn't connect GIMI: GIMI_ERROR.");
      dPrint(1,"Couldn't connect GIMI: GIMI_ERROR.");
    }

  } else {
    wxMessageBox(_T("Couldn't read connection settings file (gimi_settings.xml)."));
    dPrint(1,"Couldn't read connection settings file (gimi_settings.xml).");
  }*/
}

CGIMIConnector::~CGIMIConnector()
{
  //delete gimi;
}

void CGIMIConnector::Check()
{
  if (gimi->isConnected())
  {
    //dPrint(1,"checking gimi...");
    gimi::GIMIMessage message;
    if (gimi->receive(message,0,GIMI_PROTOCOL_FSRSIM_MOTORCMD,GIMI_IGNOREMINORTYPE)==GIMI_OK)
    {
      //dPrint(1,"Received motor command");
      if (message.datalength!=sizeof(TFSRSimMotorCommand))
      {
        dPrint(1,"Packet size does not match struct size");
        return;
      }
      TFSRSimMotorCommand *c=(TFSRSimMotorCommand *)message.getData();
      j2b2->SetSpeed(c->speed,c->angularSpeed,c->acceleration);//,c->angularAcceleration);
    }
    if (gimi->receive(message,0,GIMI_PROTOCOL_FSRSIM_POSITION,GIMI_IGNOREMINORTYPE)==GIMI_OK)
    {
     // dPrint(1,"Received position");
      if (message.datalength!=sizeof(TFSRSimPositionData))
      {
        dPrint(1,"Packet size does not match struct size");
        return;
      }
      TFSRSimPositionData *c=(TFSRSimPositionData *)message.getData();
      j2b2->SetOdoPosition(c->odometryX,c->odometryY,/*c->odometryZ,*/c->odometryHeading);
    }
  }
}

void CGIMIConnector::SendPosition(float x, float y, float z, float heading, float odox, float odoy, float odoz, float odoheading)
{
  //dPrint(1,"Sending position data");
  TFSRSimPositionData d;
  d.absoluteX=x;
  d.absoluteY=y;
  d.absoluteZ=z;
  d.absoluteHeading=heading;
  d.odometryX=odox;
  d.odometryY=odoy;
  d.odometryZ=odoz;
  d.odometryHeading=odoheading;
  d.timeStamp.setTimeMs((int)(1000*j2b2->SimWorld->GetSimulationTime()));

  gimi->sendToSubscribers((const char*)&d, sizeof(TFSRSimPositionData),GIMI_PROTOCOL_FSRSIM_POSITION,0);
  //dPrint(1,"sendtosubs:%s",gimi::getGimiErrorString(gimi->sendToSubscribers((const char*)&d, sizeof(TFSRSimPositionData),GIMI_PROTOCOL_FSRSIM_POSITION,0)).c_str());
}

void CGIMIConnector::SendRanging(int count, float *distances, float *angles, int id)
{
  //dPrint(1,"Sending ranging data");
  int size;
  const char* b;
  TFSRSimRangingData rd;
  rd.count=count;
  rd.distances=distances;
  rd.angles=angles;
  rd.timeStamp.setTimeMs((int)(1000*j2b2->SimWorld->GetSimulationTime()));
  b=(const char*)rd.createData(size);

  gimi->sendToSubscribers(b, size,GIMI_PROTOCOL_FSRSIM_RANGING,id);
  //dPrint(1,"sendtosubs:%s",gimi::getGimiErrorString(gimi->sendToSubscribers(b, size,GIMI_PROTOCOL_FSRSIM_RANGING,id)).c_str());

}
